﻿#region usings
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Text;
using System.IO;
using System.Net.Sockets;
using System.Threading;
using System.Windows.Forms;
using Microsoft.DirectX.DirectInput;
#endregion

namespace UserInterface
{

    public partial class UserForm : Form
    {
        #region globaldefs
        int displayheight = 480;
        int displaywidth = 640;
        const double vfov = 40.0 / 180.0 * Math.PI; //vertical field of view = 40 degrees
        const double hfov = 60.0 / 180.0 * Math.PI; //horizontal field of view = 60 degrees

        Microsoft.DirectX.DirectInput.Device joystick = null;
        bool latencymonitoring, speedprediction, maxspeedon, stoppingprediction, maintaineddelay = false;

        string host;
        StreamWriter sw;
        NetworkStream ns;
        double lastspeed, laststeer;
        int lastdirection;
        long lastdisplayedtime;
        int size = 32000;

        double heightofcam = 0.4; //40cm off ground
        double frontofcar = 0.2; //20cm from front of car
        double beta = 0.0; //angle from central
        double alpha = Math.PI * 24.0 / 180.0; //angle from horizontal
        double speedofcar = 0.0; //in ms-1
        double wheelangle = 0.0; //leftlock = -pi/6    right = +pi/6
        double turningradius = 2.0; //metres
        int estimatedlatency = 0; //milliseconds
        int displaylatency = 0; //milliseconds
        int oldestimate = 0;
        int count = 0;
        int threadcount;
        int x = 0;
        bool delaying = false;
        int fixeddelay = 0;

        Thread webcamcheck;
        Thread keepalive;
        int newmaxdelay, newmindelay;
        int smoothedmaxdelay, smoothedmindelay;
        int meandelay;
        Random rand = new Random();

        Queue<ImageAndTimeStamp> webimages = new Queue<ImageAndTimeStamp>();
        Queue<ImageAndTimeStamp> delayedwebimages = new Queue<ImageAndTimeStamp>();
        Queue<ImageAndTimeStamp> smoothedwebimages = new Queue<ImageAndTimeStamp>();
        #endregion

        #region constructors
        public UserForm(TcpClient tcp)
        {
            InitializeComponent();
            host = tcp.ToString();
            try
            {
                sw = new StreamWriter(tcp.GetStream());
                ns = tcp.GetStream();
            }
            catch (SocketException)
            {
                Console.WriteLine("There is no car at the given location. Exiting...");
                Close();
            }
            

            //Queue<ImageAndTimeStamp> q = Queue<ImageAndTimeStamp>.Synchronized(webimages);
            webcamcheck = new Thread(checkWebcamTCP);
            keepalive = new Thread(KeepAlive);
            webcamcheck.Start();
            keepalive.Start();
            new Thread(displayImages).Start();
        }

        public UserForm(string hostname, int port)
        {
            InitializeComponent();
            host = hostname;
            TcpClient tcp = new TcpClient();
            try
            {
                tcp.Connect(host, port);
            }
            catch (SocketException)
            {
                Console.WriteLine("There is no car at the given location. Exiting...");
                Environment.Exit(0);
            }

            sw = new StreamWriter(tcp.GetStream());
            ns = tcp.GetStream();

            webcamcheck = new Thread(checkWebcamTCP);
            keepalive = new Thread(KeepAlive);
            webcamcheck.Start();
            keepalive.Start();
            new Thread(displayImages).Start();

        }
        #endregion

        private void KeepAlive()
        {
            DateTime usertime;
            //Thread.CurrentThread.IsBackground = true;
            while (true)
            {
                try
                {
                    usertime = DateTime.Now;
                    writeOut("ALIVE:" + usertime.Ticks);
                }
                catch (IOException)
                {
                    Console.WriteLine("Connection to car lost. Exiting...");
                    Application.Exit();
                }
                Thread.Sleep(1000);
            }
        }

        void displayGearChange(int direction)
        {
            switch (direction)
                    {
                        case -1:
                            Console.WriteLine("In Gear");
                            break;
                        case 0:
                            Console.WriteLine("Neutral");
                            writeOut("KILL");
                            break;
                        case 1:
                            Console.WriteLine("Reverse");
                            break;
                    }
        }

        private void Poll()
        {
            while (true)
            {
                int direction = 0;
                //collect button states including gearstick postion
                byte[] buttons = joystick.CurrentJoystickState.GetButtons();
                if (buttons[8] == 128) //reverse gear selected
                {
                    direction = -1;
                }
                else if (buttons[14] == 128) //forwards gear selected
                {
                    direction = 1;
                }
                else //not in a valid gear = 'neutral'
                {
                    direction = 0;
                }

                //inform user if gear change has occurred
                if (lastdirection != direction)
                {
                    displayGearChange(direction);
                    lastdirection = direction;
                }

                //calculate speed coefficient using accelerator/brake slider value
                double speed = Math.Round((((joystick.CurrentJoystickState.GetSlider()[0] / 65535.0)) - 0.5) * 2, 2);
                Console.WriteLine("Slider: " + speed);

                //calculate steer coefficient using wheel (X) position
                double steer = Math.Round((((joystick.CurrentJoystickState.X / 65535.0) * 2) - 1), 2);

                if (direction != 0)
                {
                    //not in neutral so need to write command to car

                    if (laststeer != steer) //avoid writing same command twice (reduce network load)
                    {
                        writeOut("STEER:" + steer);
                        //update wheel angle and camera pan for stopping point
                        wheelangle = calculateWheelAngle(steer);
                        beta = (Math.PI / 4) * steer * 0.25;// beta = current camera pan
                    }
                    laststeer = steer;

                    //repeat for speed coefficient
                    if (lastspeed != speed)
                    {
                        writeOut("SPEED:" + direction * speed);
                        speedofcar = calculateSpeedOfCar(direction * speed);
                    }
                    lastspeed = speed;
                }
                else //in neutral
                {
                    //replace SPEED with TILT
                    //replace STEER with PAN
                    if (laststeer != steer)
                    {
                        writeOut("PAN:" + steer);
                        beta = (Math.PI / 4) * steer * 0.25;
                    }
                    laststeer = steer;

                    if (lastspeed != speed)
                    {
                        writeOut("TILT:" + speed);
                    }
                    lastspeed = speed;
                }
                Thread.Sleep(50); //joystick polled 20 times/sec
            }
        }

        #region webcamstuff
        private void checkWebcamTCP()
        {
            DateTime time = new DateTime();
            time = DateTime.Now;
            byte[] bytes = new byte[size];
            while (true)
            {
                Thread.Sleep(40); //NEEDED
                try
                {

                    int x = ns.Read(bytes, 0, size);
                    if (speedprediction)
                    {
                        label14.Invoke(new UpdateTextCallback(this.updateLabel14), "SPEED = " + Math.Round((speedofcar * 2.24),2) + " mph.");
                    }
                    ImageAndTimeStamp temp = byteArrayToImageAndTimeStamp(bytes);
                    webimages.Enqueue(temp);
                    new Thread(delayWebcam).Start();
                }
                catch (Exception)
                {
                    //most likely hiccup in network stream - carry on
                }

            }
        }

        private void delayWebcam()
        {
            if (delaying)
            {
                int randy = rand.Next(200);
                x = x + randy - 100;
                x = Math.Min(1500, x);
                x = Math.Max(500, x);
                /*if (maintaineddelay)
                {
                    x = 1000;
                }*/
                //Console.WriteLine(x);
            }
            ImageAndTimeStamp imgandtime;
            Image temp;
            Bitmap bmp;
            Image.GetThumbnailImageAbort dummyCallBack = new Image.GetThumbnailImageAbort(ThumbnailCallback);
            //Thread.Sleep(40);
            try
            {
                if (webimages.Count != 0)
                {
                    lock (webimages)
                    {
                        imgandtime = webimages.Dequeue();
                    }
                    temp = imgandtime.img;
                    bmp = (Bitmap)temp.GetThumbnailImage(640, 480, dummyCallBack, IntPtr.Zero);
                    if (delaying)
                    {
                        while (imgandtime.timestamp - DateTime.Now.Ticks + x * 10000 > 0)
                        {
                            Thread.Sleep(50);
                        }
                        
                    }
                    lock(delayedwebimages)
                        {
                            delayedwebimages.Enqueue(imgandtime);
                        }
                    new Thread(smoothDelays).Start();
                }
            }
            catch
            {
            }
        }

        void displayImages()
        {
            ImageAndTimeStamp imgandtime;
            Image temp;
            Bitmap bmp;
            Image.GetThumbnailImageAbort dummyCallBack = new Image.GetThumbnailImageAbort(ThumbnailCallback);
            while(true)
            {
                try
                {
                Thread.Sleep(40);
                lock (smoothedwebimages)
                {
                    if (smoothedwebimages.Count != 0)
                    {

                        imgandtime = smoothedwebimages.Dequeue();
                        temp = imgandtime.img;
                        bmp = (Bitmap)temp.GetThumbnailImage(640, 480, dummyCallBack, IntPtr.Zero);
                        if (stoppingprediction)
                        {
                            pictureBox1.Image = DrawOnDots(bmp);
                        }
                        else
                        {
                            pictureBox1.Image = bmp;
                        }

                    }
                }
                }
                catch(Exception e)
                {
                    Console.WriteLine(e.StackTrace);
                }
            }
        }

        void smoothDelays()
        {
            threadcount++; //used to check how many images are being delayed 
            ImageAndTimeStamp imgandtime;
            try
            {
                if (delayedwebimages.Count != 0) //buffer is not empty
                {
                    lock (delayedwebimages)
                    {
                        //remove oldest image from the head of the input queue
                        imgandtime = delayedwebimages.Dequeue(); 
                    }
                    //if the fixed delay mechanism is on
                    //then need to add at least 'fixeddelay' milliseconds
                    //otherwise just carry on
                    if (maintaineddelay)
                    {
                        while (imgandtime.timestamp - DateTime.Now.Ticks + fixeddelay * 10000 > 0)
                        {
                            Thread.Sleep(100);
                        }
                    }

                    //check that image is not out of date
                    if (imgandtime.timestamp < lastdisplayedtime)
                    {
                        //discard image as out of order
                    }
                    else 
                    {
                        //image is in order so must be displayed
                        //calculate current display latency and add to output queue
                        displaylatency = (int)(DateTime.Now.Ticks - imgandtime.timestamp) / 10000;
                        lock (smoothedwebimages)
                        {
                            smoothedwebimages.Enqueue(imgandtime);
                        }
                        //update most recent timestamp
                        lastdisplayedtime = imgandtime.timestamp;
                    }
                    //display current delay to the user
                    label2.Invoke(new UpdateTextCallback(this.updateLabel2), "Queue Length: " + delayedwebimages.Count + " Thread Count: " + threadcount);
                }
            }
            catch (Exception ex)
            {
                Console.WriteLine(ex.StackTrace);
            }
            threadcount--;
        }

        private void changeResolution(int factor)
        {
            double f = factor / 10.0;
            double h = displayheight * f;
            double w = displaywidth * f;
            h = Math.Max(32, h);
            w = Math.Max(24, w);
            Console.WriteLine(h + " " + w);
            writeOut("HEIGHT:" + h);
            writeOut("WIDTH:" + w);
        }
        private void updateLabel2(string text)
        {
            label2.Text = text;
            toolStripStatusLabel1.Text = text;
        }
        private void updateLabel9(string text)
        {
            label9.Text = text;
            toolStripStatusLabel2.Text = text;
        }
        private void updateLabel10(string text)
        {
            label10.Text = text;
            toolStripStatusLabel3.Text = text;
        }
        private void updateLabel12(string text)
        {
            label12.Text = text;
            toolStripStatusLabel4.Text = text;
        }
        private void updateLabel14(string text)
        {
            label14.Text = text;
            toolStripStatusLabel5.Text = text;
        }
        public delegate void UpdateTextCallback(string text);

        private Bitmap DrawOnDots(Bitmap b)
        {
            double distance = estimatedlatency * speedofcar / 1000;
            double forward = distanceForward(distance);
            double left = distanceLeft(distance);
            double angledown = angleDownFromCentral(forward, left) - alpha;
            double angleleft = angleLeftFromCentral(forward, left) - beta;
            Console.Clear();
            Console.WriteLine("Distance: " + distance);
            Console.WriteLine("Forward: " + forward);
            Console.WriteLine("Left: " + left);
            Console.WriteLine("Angle Down: " + angledown);
            Console.WriteLine("Angle Left: " + angleleft);

            double pixelsleft = angleleft / hfov * displaywidth;
            double pixelsdown = angledown / vfov * displayheight;
            int x = displaywidth / 2 + (int)Math.Floor(pixelsleft);
            int y = displayheight / 2 + (int)Math.Floor(pixelsdown);

            if (x <= 0 || x > displaywidth - 1)
            { }
            else if (y <= 0 || y > displayheight - 1)
            {
            }
            else
            {
                for (int i = -2; i < 3; i++)
                {
                    for (int j = -2; j < 3; j++)
                    {
                        b.SetPixel(x + i, y + j, Color.Yellow);
                    }
                }
            }
            return b;

        }

        public bool ThumbnailCallback()
        {
            return false;
        }

        public ImageAndTimeStamp byteArrayToImageAndTimeStamp(byte[] byteArrayIn)
        {
            //extract first 8 bytes and convert to a time value
            byte[] timebytes = new byte[8];
            Array.Copy(byteArrayIn, 0, timebytes, 0, 8);
            long time = BitConverter.ToInt64(timebytes, 0);

            if (latencymonitoring)
            {
                oldestimate = estimatedlatency;
                //estimate = NOW - extracted timestamp + (deliberate delay)
                estimatedlatency = (int)(DateTime.Now.Ticks - DateTime.FromBinary(time).Ticks) / 10000;
                estimatedlatency += x;
                if (estimatedlatency < 0 || estimatedlatency > 3000)
                {
                    estimatedlatency = oldestimate;
                }
                //update maximum and minimum recent delays 
                calculateDelays(estimatedlatency);

                //for every 10 new images, inform user of display delays
                count++;
                if (count == 10)
                {
                    label10.Invoke(new UpdateTextCallback(this.updateLabel10), displaylatency + " display delay.");
                    label9.Invoke(new UpdateTextCallback(this.updateLabel9), estimatedlatency + " milliseconds delay.");
                    count = 0;
                }
            }

            //convert image bytes back to an image 
            //and store time and image in one object
            MemoryStream ms = new MemoryStream(byteArrayIn, 8, size - 8);
            Image returnImage = Image.FromStream(ms, true, false);
            ImageAndTimeStamp imgandtime = new ImageAndTimeStamp(returnImage, DateTime.FromBinary(time).Ticks);
            return imgandtime;
        }

        #endregion

        #region distancecalcs
        private double calculateSpeedOfCar(double x)
        {
            if (x < -0.3)
                return x * 1.7 - 0.3;
            else if (x > 0.3)
                return x * 1.3 - 0.07;
            else return 0;
        }

        private void testCommandLoop()
        {
            int numbersent;
            for (int tosend = 0; tosend < 1000; tosend+=10)
            {
                writeOut("Current output: " + f);
                numbersent = 0;
                while (numbersent < tosend)
                {
                    
                    writeOut("SPEED:" + (rand.Next(10)));
                    Thread.Sleep(1000 / tosend);
                    numbersent++;
                }
            }
        }


        private double calculateWheelAngle(double x)
        {
            return x * Math.PI / 6;
        }

        private double distanceForward(double distancetravelled)
        {
            //needs carspeed, wheel angle, estimated latency
            return distancetravelled * Math.Cos(wheelangle);
        }

        private double distanceLeft(double distancetravelled)
        {
            return distancetravelled * Math.Sin(wheelangle);
        }

        private double distanceTravelled()
        {
            double angularvelocity = speedofcar / turningradius;
            //needs carspeed, wheel angle, estimated latency
            return turningradius * Math.Sin(angularvelocity * estimatedlatency) / (Math.Sin(Math.PI / 2 * angularvelocity * estimatedlatency));
        }

        private double angleDownFromCentral(double distforward, double distleft)
        {
            double x = frontofcar + distforward;
            return Math.Atan2(heightofcam, x);
        }

        private double angleLeftFromCentral(double distforward, double distleft)
        {
            double x = frontofcar + distforward;
            return Math.Atan2(distleft, x);
        }
        #endregion

        #region interfacebuttons

        private void button1_Click(object sender, EventArgs e)
        {
            writeOut("KILL");
        }

        private void label1_MouseMove(object sender, MouseEventArgs e)
        {
            //LEFT MOUSE = change speed and direction
            //RIGHT MOUSE = change pan and tilt of camera
            if (e.Button == MouseButtons.Left)
            {
                double steer = Math.Round((e.X / (double)label1.Width) * 2 - 1, 2);
                double speed = Math.Round(-(((e.Y / (double)label1.Height) - 0.5) * 2), 2);
                if (maxspeedon)
                {
                    speed = Math.Round(maxSpeed(speed), 2);
                }
                //write out to the car
                writeOut("STEER:" + steer);
                writeOut("SPEED:" + speed);
                //set current speed of car
                speedofcar = calculateSpeedOfCar(speed);
                wheelangle = calculateWheelAngle(steer);
                //double speedTilt = Math.Max(0, speed - 0.3) * 0.35 - 0.35;
                //alpha = (Math.PI / 4) * speedTilt;
                beta = (Math.PI / 4) * steer * 0.25;

            }
            else if (e.Button == MouseButtons.Right)
            {
                double pan = Math.Round((e.X / (double)label1.Width) * 2 - 1, 2);
                double tilt = Math.Round(-((e.Y / (double)label1.Height) * 2 - 1), 2);
                //write out to car
                writeOut("PAN:" + pan);
                writeOut("TILT:" + tilt);
                //calculate new angles for camera view cone
                //alpha = tilt angle (radians) 0 = horizontal
                //beta = pan angle (radians) 0 = central
                //alpha = (Math.PI / 4) * tilt;
                beta = (Math.PI / 4) * pan;
            }
        }

        private void label1_MouseUp(object sender, MouseEventArgs e)
        {
            //MOUSE RELEASE = STOP
            writeOut("KILL");
            speedofcar = 0;
            //alpha = beta = 0;
            label1.Capture = false;
            //stop clipping box
            Cursor.Clip = Rectangle.Empty;
        }

        private void label1_MouseDown(object sender, MouseEventArgs e)
        {
            Cursor.Position = label1.PointToScreen(new Point((int)(label1.Width / 2), label1.Height / 2));
            Cursor.Clip = new Rectangle(label1.PointToScreen(new Point(0, 0)), new Size(label1.Width, label1.Height));
        }

        private void toggleJoystick_Click(object sender, EventArgs e)
        {
            if (joystick == null)
            {
                detectJoystick();
                label3.BackColor = Color.Green;
                label3.Text = "ON";
            }
        }

        private void button1_Click_1(object sender, EventArgs e)
        {//latency monitoring button

            if (latencymonitoring)
            {
                label4.BackColor = Color.OrangeRed;
                label4.Text = "OFF";
                latencymonitoring = false;
                label10.Invoke(new UpdateTextCallback(this.updateLabel10), "");
                label9.Invoke(new UpdateTextCallback(this.updateLabel9), "");
            }
            else
            {
                label4.BackColor = Color.Green;
                label4.Text = "ON";
                latencymonitoring = true;
            }
        }

        #endregion

        private void writeOut(string s)
        {
            sw.WriteLine(s);
            sw.Flush();
            if (s.StartsWith("ALIVE"))
            {
            }
            else Console.WriteLine("SENT: " + s);
        }

        private void detectJoystick()
        {
            try
            {
                DeviceList dl = Manager.GetDevices(DeviceClass.GameControl, EnumDevicesFlags.AttachedOnly);

                while (dl.MoveNext())
                {
                    var d = (DeviceInstance)dl.Current;
                    joystick = new Device(d.InstanceGuid);
                    break;
                }
                if (joystick != null)
                {
                    joystickButton.Visible = true;
                    Console.WriteLine(joystick.DeviceInformation);
                    joystick.Acquire();
                    new Thread(Poll).Start();
                }
            }
            catch
            {
                MessageBox.Show("There was no steering wheel detected.");
            }
        }

        private void UserForm_Load(object sender, EventArgs e)
        {

        }

        private void speedPredictionButton_Click(object sender, EventArgs e)
        {
            if (speedprediction)
            {
                label5.BackColor = Color.OrangeRed;
                label5.Text = "OFF";
                speedprediction = false;
                label14.Invoke(new UpdateTextCallback(this.updateLabel14), "");
            }
            else
            {
                label5.BackColor = Color.Green;
                label5.Text = "ON";
                speedprediction = true;
            }
        }

        private void speedLimitButton_Click(object sender, EventArgs e)
        {
            if (maxspeedon)
            {
                label6.BackColor = Color.OrangeRed;
                label6.Text = "OFF";
                maxspeedon = false;
            }
            else
            {
                label6.BackColor = Color.Green;
                label6.Text = "ON";
                maxspeedon = true;
            }
        }

        private void stoppingPredictionButton_Click(object sender, EventArgs e)
        {
            if (stoppingprediction)
            {
                label7.BackColor = Color.OrangeRed;
                label7.Text = "OFF";
                stoppingprediction = false;
            }
            else
            {
                label7.BackColor = Color.Green;
                label7.Text = "ON";
                stoppingprediction = true;
            }
        }

        private void maintainDelayButton_Click(object sender, EventArgs e)
        {
            if (maintaineddelay)
            {
                label8.BackColor = Color.OrangeRed;
                label8.Text = "OFF";
                maintaineddelay = false;
                fixeddelay = 0;
            }
            else
            {
                label8.BackColor = Color.Green;
                label8.Text = "ON";
                maintaineddelay = true;
                //fixeddelay = 1000;//int.Parse(textBox1.Text);
            }
        }

        private void UserForm_FormClosed(object sender, FormClosedEventArgs e)
        {
            webcamcheck.Abort();
            keepalive.Abort();
            //webcamdisplay.Abort();
            Application.Exit();
        }

        private double maxSpeed(double speed)
        {
            if (estimatedlatency < 500)
            {
                return speed;
            }
            else if (estimatedlatency > 2000)
            {
                return speed * 0.4;
            }
            else return speed * (-0.4 * estimatedlatency + 1700) / 1500;
        }

        private void calculateDelays(int estimate)
        {
            //estimate a sensible amount for maximum delay
            //using exponential smoothing function
            double a = 0.99;
            newmaxdelay = Math.Max(estimate, smoothedmaxdelay);
            newmindelay = Math.Min(estimate, smoothedmindelay);

            smoothedmaxdelay = (int)Math.Round((1 - a) * newmaxdelay + a * smoothedmaxdelay);
            smoothedmindelay = (int)Math.Round((1 - a) * newmindelay + a * smoothedmindelay);

            fixeddelay = (int)(0.6 * (smoothedmaxdelay + smoothedmindelay));
        }

        private void button1_Click_2(object sender, EventArgs e)
        {
            if (delaying)
            {
                label11.BackColor = Color.OrangeRed;
                label11.Text = "OFF";
                delaying = false;
                x = 0;
            }
            else
            {
                label11.BackColor = Color.Green;
                label11.Text = "ON";
                delaying = true;
                x = 1000;
            }
        }

        private void trackBar1_Scroll(object sender, EventArgs e)
        {
            Console.WriteLine(trackBar1.Value);
            changeResolution(trackBar1.Value);
        }

        private void button3_Click(object sender, EventArgs e) //grey button - all off (no latency)
        {
            latencymonitoring = delaying = maintaineddelay = maxspeedon = speedprediction = stoppingprediction = false;
            label11.BackColor = Color.OrangeRed;
            label11.Text = "OFF";
            label4.BackColor = Color.OrangeRed;
            label4.Text = "OFF";
            label5.BackColor = Color.OrangeRed;
            label5.Text = "OFF";
            label6.BackColor = Color.OrangeRed;
            label6.Text = "OFF";
            label7.BackColor = Color.OrangeRed;
            label7.Text = "OFF";
            label8.BackColor = Color.OrangeRed;
            label8.Text = "OFF";
            this.BackColor = Color.LightGray;
            label14.Invoke(new UpdateTextCallback(this.updateLabel14), "");
            label10.Invoke(new UpdateTextCallback(this.updateLabel10), "");
            label9.Invoke(new UpdateTextCallback(this.updateLabel9), "");
        }

        private void button4_Click(object sender, EventArgs e) //green - ALL ON
        {
            latencymonitoring = delaying = maintaineddelay = maxspeedon = speedprediction = stoppingprediction = true;
            //fixeddelay = 1000;
            textBox1.Text = "1000";
            label11.BackColor = Color.Green;
            label11.Text = "ON";
            label4.BackColor = Color.Green;
            label4.Text = "ON";
            label5.BackColor = Color.Green;
            label5.Text = "ON";
            label6.BackColor = Color.Green;
            label6.Text = "ON";
            label7.BackColor = Color.Green;
            label7.Text = "ON";
            label8.BackColor = Color.Green;
            label8.Text = "ON";
            this.BackColor = Color.LightGreen;
            ;
        }

        private void button5_Click(object sender, EventArgs e) //orange - some on
        {
            latencymonitoring = delaying = maxspeedon = speedprediction = true;
            stoppingprediction = maintaineddelay = false;
            textBox1.Text = "";
            label11.BackColor = Color.Green;
            label11.Text = "ON";
            label4.BackColor = Color.Green;
            label4.Text = "ON";
            label5.BackColor = Color.Green;
            label5.Text = "ON";
            label6.BackColor = Color.Green;
            label6.Text = "ON";
            label7.BackColor = Color.OrangeRed;
            label7.Text = "OFF";
            label8.BackColor = Color.OrangeRed;
            label8.Text = "OFF"; ;
            this.BackColor = Color.Orange;
        }

        private void button6_Click(object sender, EventArgs e) //purple - all off
        {
            latencymonitoring = maintaineddelay = maxspeedon = speedprediction = stoppingprediction = false;
            delaying = true;
            label11.BackColor = Color.Green;
            label11.Text = "ON";
            label4.BackColor = Color.OrangeRed;
            label4.Text = "OFF";
            label5.BackColor = Color.OrangeRed;
            label5.Text = "OFF";
            label6.BackColor = Color.OrangeRed;
            label6.Text = "OFF";
            label7.BackColor = Color.OrangeRed;
            label7.Text = "OFF";
            label8.BackColor = Color.OrangeRed;
            label8.Text = "OFF";
            this.BackColor = Color.MediumPurple;
            label14.Invoke(new UpdateTextCallback(this.updateLabel14), "");
            label10.Invoke(new UpdateTextCallback(this.updateLabel10), "");
            label9.Invoke(new UpdateTextCallback(this.updateLabel9), "");
        }

        private void trackBar2_Scroll(object sender, EventArgs e)
        {
            writeOut("STEERFACTOR:" + (double)(trackBar2.Value - 5) / 40.0);
        }

        private void trackBar3_Scroll(object sender, EventArgs e)
        {
            writeOut("TILTFACTOR:" + (double)((trackBar3.Value - 5) / 40.0 - 0.1));
        }

        private void button7_Click(object sender, EventArgs e)
        {
            testCommandLoop();
        }

    }

    public class ImageAndTimeStamp
    {
        public Image img;
        public long timestamp;

        public ImageAndTimeStamp(Image i, long t)
        {
            img = i;
            timestamp = t;
        }
    }
}
